"""
回固定点模块
"""
import logging
import time

import gevent
import qte

from hy_four_channels.hal.hand.hand_operator_all_ch import CHANNEL_NAME_LIST
from hy_four_channels.hal.hand.hand_operator_all_ch import MoveTuple
from hy_four_channels.hal.hand.hand_operator_all_ch import get_axis_id

from hy_four_channels.hal.runtime.processdata import write_process_data
from hy_four_channels.hal.runtime.processdata import read_process_data

from hy_four_channels.hal.hand.st_data_type import E_MC_BufferMode_T
from hy_four_channels.hal.runtime.spindle_distance_check import enable_plc_spindle_distance_check
import hy_four_channels.hal.hand.mode_change as hand_mode_change
import hy_four_channels.hal.hand.hand_operator_all_ch as hand_operator_all_ch
import hy_four_channels.logic.common.spindle_position as spindle_position
from hy_four_channels.hal.hand.syncode_manager import HANDMOVESYNCODE


from stone_cnc.realtime.realtimecnc import cnc_get_feed_override
from stone_cnc.realtime.realtimecnc import cnc_get_position
from stone_cnc.realtime.realtimecnc import cnc_update_go_fixed_point_state
from stone_cnc.parameter.parameter import read_spindle_distance_parameter
from stone_cnc.parameter.parameter import read_manual_parameter
from stone_cnc.realtime.ncaxisconfig import nc_axis_name_from_index

_DEBUG = logging.getLogger(__name__).debug

_MM_MIN = qte.unit.mm / qte.unit.minute
_M_S_2 = qte.unit.m / (qte.unit.second ** 2)
_M_S_3 = qte.unit.m / (qte.unit.second ** 3)

# pylint: disable=W1202

_AXIS_INDEX_MAP = {
    "X": 0,
    "Y": 1,
    "Z": 2
}


def _is_reach_position(target_positions, axis_name):
    for _i, channel_name in enumerate(CHANNEL_NAME_LIST):
        _current_pos = cnc_get_position("{}.{}".format(
            channel_name, axis_name))
        if abs(_current_pos - target_positions[_i][_AXIS_INDEX_MAP[axis_name]]) >= (10 ** -6):
            return False

    return True


def _is_reach_position_single_axis(target_position, axis_name):
    _current_pos = cnc_get_position(axis_name)
    if abs(_current_pos - target_position) < (10 ** -6):
        return True
    else:
        return False


def _calc_time_consume(distance, speed):
    if cnc_get_feed_override() != 100:
        _DEBUG("inc = {}, time = {}".format(
            (abs(distance) / qte.unit.mm), 3600))
        return 3600  # 如果用户修改了进给倍率 时间定为3600s

    _t = (abs(distance) / qte.unit.mm) / speed
    _t += 0.5 * _t
    if _t < 5:
        _t = 5
    _DEBUG("inc = {}, time = {}".format((abs(distance) / qte.unit.mm), _t))
    return _t


def _calc_target_positions(start_channel, channel_count, x_inc, y_inc, z_inc=None):
    """计算当前轴的目标位置"""
    _positions = spindle_position.get_spindle_positions()

    # 当前的机床位置 + 对应轴的增量 => 运动的目标位置
    for i in range(start_channel, channel_count + 1):
        _index = i - 1

        if x_inc is not None and x_inc != 0:
            _positions[_index][0] += x_inc

        if y_inc is not None and y_inc != 0:
            _positions[_index][1] += y_inc

        if z_inc is not None and z_inc != 0:
            _positions[_index][2] += z_inc

    return _positions


def _move_channels(start_channel, channel_count, x_inc, y_inc):
    """
    从start_channel开始移动后面的所有主轴的x，y
    """
    result = []
    _vec = read_manual_parameter("go_fixed_point_velocity") / _MM_MIN
    _acc = read_manual_parameter("go_fixed_point_acceleration") / _M_S_2
    _dec = read_manual_parameter("go_fixed_point_deceleration") / _M_S_2
    _jerk = read_manual_parameter("go_fixed_point_jerk") / _M_S_3

    # 用默认参数构造要发送的数据结构
    for _index, _chname in enumerate(CHANNEL_NAME_LIST):
        # _syncode说明，偶数表示运动，奇数表示停止
        _syncode = HANDMOVESYNCODE.start_syncode(_index)
        result.append(MoveTuple._make([-1,
                                       _syncode,
                                       0,
                                       _vec,
                                       _acc,
                                       _dec,
                                       _jerk,
                                       E_MC_BufferMode_T.MC_Buffered,
                                       False])._asdict())

    # 计算当前要运动到的目标位置, 用于到位检测
    _target_positions = _calc_target_positions(
        start_channel, channel_count, x_inc, y_inc)

    # 填充要运动的x轴的相应字段
    if x_inc != 0.0:
        for i in range(start_channel, channel_count + 1):
            _ch_name = "ch%d" % i
            _axis_id = get_axis_id(_ch_name, 'X')
            _DEBUG("{}, x, {}, {}".format(
                _ch_name, _axis_id, x_inc / qte.unit.mm))
            result[i - 1]["AXISID"] = _axis_id
            result[i - 1]["POSITION"] = x_inc / qte.unit.mm

        # 估算当前运动量所需的大概时间, 用于等待超时
        _x_inc_time_consume = _calc_time_consume(x_inc, _vec)

        # 将运动指令发给plc
        send_struct = {"CHANNEL_MANUAL_MOVE": result}
        write_process_data("manual_move_all_channel", send_struct)

        # 等待plc控制对应的轴，到指定的位置
        _start_time = time.time()
        while True:
            # 判断是否超时
            if time.time() - _start_time >= _x_inc_time_consume:
                raise TimeoutError("wait x to position time out")

            # 判断是否到达了指定的位置
            if _is_reach_position(_target_positions, "X"):
                gevent.sleep(0.3)
                break
            gevent.sleep(0.2)

    if y_inc != 0.0:
        for i in range(start_channel, channel_count + 1):
            _ch_name = "ch%d" % i
            _axis_id = get_axis_id(_ch_name, 'Y')
            _DEBUG("{}, y, {}, {}".format(
                _ch_name, _axis_id, y_inc / qte.unit.mm))
            result[i - 1]["AXISID"] = _axis_id
            result[i - 1]["POSITION"] = y_inc / qte.unit.mm
            _syncode = HANDMOVESYNCODE.start_syncode(i-1)
            result[i - 1]["SYNCODE"] = _syncode

        _y_inc_time_consume = _calc_time_consume(y_inc, _vec)

        send_struct = {"CHANNEL_MANUAL_MOVE": result}
        write_process_data("manual_move_all_channel", send_struct)

        _start_time = time.time()
        while True:
            if time.time() - _start_time >= _y_inc_time_consume:
                raise TimeoutError("wait y to position time out")
            if _is_reach_position(_target_positions, "Y"):
                gevent.sleep(0.2)
                break
            gevent.sleep(0.2)


def _move_channels_different_z(target_positions, z_incs):
    _vec = read_manual_parameter("go_fixed_point_velocity") / _MM_MIN
    _acc = read_manual_parameter("go_fixed_point_acceleration") / _M_S_2
    _dec = read_manual_parameter("go_fixed_point_deceleration") / _M_S_2
    _jerk = read_manual_parameter("go_fixed_point_jerk") / _M_S_3
    result = []
    _is_moved = False
    for _index, _chname in enumerate(CHANNEL_NAME_LIST):
        _axis_id = get_axis_id(_chname, 'Z')

        # 这里如果运动量为0那么不发给plc
        if z_incs[_index] != 0.0:
            _DEBUG("{}, z, {}, {}".format(
                _chname, _axis_id, z_incs[_index] / qte.unit.mm))
            _is_moved = True
            _syncode = HANDMOVESYNCODE.start_syncode(_index)
            result.append(MoveTuple._make([_axis_id,
                                           _syncode,
                                           z_incs[_index] / qte.unit.mm,
                                           _vec,
                                           _acc,
                                           _dec,
                                           _jerk,
                                           E_MC_BufferMode_T.MC_Buffered,
                                           False])._asdict())
        else:
            result.append(MoveTuple._make([-1,
                                           HANDMOVESYNCODE.current_syncode(
                                               _index),
                                           0,
                                           _vec,
                                           _acc,
                                           _dec,
                                           _jerk,
                                           E_MC_BufferMode_T.MC_Buffered,
                                           False])._asdict())

    # 如果没有z轴运动的话，那么不需要检测是否到位
    if _is_moved:

        # 发送运动指令
        send_struct = {"CHANNEL_MANUAL_MOVE": result}
        write_process_data("manual_move_all_channel", send_struct)

        # 逐个判断四个z轴是否都到位
        for _index, _pos in enumerate(target_positions):
            _inc_time_consume = _calc_time_consume(z_incs[_index], _vec)
            _start_time = time.time()
            while True:
                if time.time() - _start_time >= _inc_time_consume:
                    raise TimeoutError("wait z to position time out")
                _axis_name = "ch%d.Z" % (_index + 1)
                if _is_reach_position_single_axis(_pos, _axis_name):
                    break
                else:
                    gevent.sleep(0.2)

        gevent.sleep(0.2)


def _calc_x_y_move(expect_x, expect_y, real_x, real_y):
    """
    计算恢复到设定的主轴间距，x和y方向需要走多远
    """
    return (expect_x - real_x, expect_y - real_y)


def _reset_spindle_distance(spindle_positions,
                            spindle_distances,
                            complete_callback=None):
    """
    移动主轴使得其满足，用户设定的主轴间距
    """
    if complete_callback:
        hand_mode_change.push_mode()
        hand_mode_change.change_to_hand_inc()
        while True:
            if hand_mode_change.current_is_hand_inc():
                gevent.sleep(0.1)
                break
            gevent.sleep(0.1)

    for i in range(2, len(spindle_positions) + 1):
        # 读取用户期望的主轴间距
        _expect_distance_x = spindle_distances[i - 2][0]
        _expect_distance_y = spindle_distances[i - 2][1]

        # 计算当前实际的主轴间距
        _real_distance_x = spindle_positions[i - 1][0] - \
            spindle_positions[i - 2][0]
        _real_distance_y = spindle_positions[i - 1][1] - \
            spindle_positions[i - 2][1]

        # 计算要调整到，目标间距，所需要的运动量
        _x_inc, _y_inc = _calc_x_y_move(
            _expect_distance_x, _expect_distance_y, _real_distance_x, _real_distance_y)
        if _x_inc == 0 and _y_inc == 0:
            continue

        _move_channels(i, len(spindle_positions), _x_inc, _y_inc)

    if complete_callback:
        hand_mode_change.pop_mode()
        complete_callback()


def _get_z_fixed_points():
    _names = ["fixed_point_z1", "fixed_point_z2",
              "fixed_point_z3", "fixed_point_z4"]
    _result = []
    for name in _names:
        _result.append(read_spindle_distance_parameter(name))

    _DEBUG("Z fixed Point = {}".format(_result))
    return _result


def _get_x_y_fixed_points():
    _names = ["fixed_point_x", "fixed_point_y"]
    _result = []
    for name in _names:
        _result.append(read_spindle_distance_parameter(name))

    _DEBUG("zy fixed Point = {}".format(_result))
    return _result


def _z_go_fixed_point(spindle_positions):
    _z_f_p = _get_z_fixed_points()

    z1_inc = _z_f_p[0] - spindle_positions[0][2]
    z2_inc = _z_f_p[1] - spindle_positions[1][2]
    z3_inc = _z_f_p[2] - spindle_positions[2][2]
    z4_inc = _z_f_p[3] - spindle_positions[3][2]
    _incs = [z1_inc, z2_inc, z3_inc, z4_inc]
    _move_channels_different_z(_z_f_p, _incs)


def _go_fixed_point_spawn(spindle_positions, spindle_distances, complete_callback):
    global GO_FIXED_POINT_SPAWN
    global GO_FIXED_POINT_STATE
    _DEBUG("Spindle_position = {}".format(spindle_positions))
    _DEBUG("spindle_distances = {}".format(spindle_distances))

    GO_FIXED_POINT_STATE = -1
    # 设置当前手动模块为增量模式
    # 并保存上一次的模式，以供后面恢复
    hand_mode_change.push_mode()
    hand_mode_change.change_to_hand_inc()

    # 等待模式切换完成
    while True:
        if hand_mode_change.current_is_hand_inc():
            break
        gevent.sleep(0.1)

    try:
        # 让各通道的z轴回到指定的位置
        _z_go_fixed_point(spindle_positions)

        # 稍微回退一点儿x轴的位置
        x_inch = read_spindle_distance_parameter("x_max_inching")
        _x_y_f_p = _get_x_y_fixed_points()
        x_inc = (_x_y_f_p[0] + x_inch) - spindle_positions[0][0]
        y_inc = _x_y_f_p[1] - spindle_positions[0][1]
        _move_channels(1, len(spindle_positions), x_inc, y_inc)

        # 调整各个主轴之间的间距
        _reset_spindle_distance(spindle_positions, spindle_distances)

        _spindle_positions = spindle_position.get_spindle_positions()
        # 四个通道一起动，目的是让通道1的x轴，运动到用户设定的位置
        _x_y_f_p = _get_x_y_fixed_points()
        x_inc = _x_y_f_p[0] - _spindle_positions[0][0]
        y_inc = _x_y_f_p[1] - _spindle_positions[0][1]
        _move_channels(1, len(_spindle_positions), x_inc, y_inc)

        # 通知plc开启主轴间距的检测
        enable_plc_spindle_distance_check(True)

        # 更新当前系统的状态为，已回固定点
        cnc_update_go_fixed_point_state(True)

    except TimeoutError as e:
        GO_FIXED_POINT_STATE = 1
        _DEBUG("{}".format(e))

    if GO_FIXED_POINT_STATE == -1:
        GO_FIXED_POINT_STATE = 0
    # 恢复手动模块的属性
    hand_mode_change.pop_mode()

    # 调用注册的回调接口
    complete_callback()

    GO_FIXED_POINT_SPAWN = None


def reset_spindle_distance(spindle_positions, spindle_distances, complete_callback):
    """
    移动主轴使得其满足，用户设定的主轴间距
    spindle_positions : 开始回固定点那一瞬间，当前机床的位置
    spindle_distances : 用户设定的主轴间距
    complete_callback : 回固定点逻辑执行完之后，调用的回调函数
    """
    gevent.spawn(_reset_spindle_distance, spindle_positions,
                 spindle_distances, complete_callback)


GO_FIXED_POINT_SPAWN = None
GO_FIXED_POINT_STATE = 0

def is_current_busy():
    """当前回固定点是否在工作"""
    if GO_FIXED_POINT_SPAWN is None:
        return False
    else:
        return True

# def demo():
#     while True:
#         if is_current_busy():
#             gevent.sleep(0.01)
#         else:
#             break
    
#     if get_go_fixed_state() == 0:
#         # 执行回固定点成功后的逻辑
#         pass
#     else:
#         # 执行回固定点失败后的逻辑
#         pass

def get_go_fixed_state():
    """返回：-1表示正忙 0表示成功， 1表示超时， 2表示用户强制结束"""
    global GO_FIXED_POINT_STATE
    return GO_FIXED_POINT_STATE

def go_fixed_point(spindle_positions, spindle_distances, complete_callback):
    """
    回固定点
    spindle_positions : 开始回固定点那一瞬间，当前机床的位置
    spindle_distances : 用户设定的主轴间距
    complete_callback : 回固定点逻辑执行完之后，调用的回调函数
    """
    global GO_FIXED_POINT_SPAWN  # pylint: disable=W0603

    GO_FIXED_POINT_SPAWN = gevent.spawn(_go_fixed_point_spawn, spindle_positions,
                                        spindle_distances, complete_callback)


def _read_current_move_axis_id(axis_move_info_list):
    for _v in axis_move_info_list:
        if _v["AXISID"] != 4294967295:
            return _v["AXISID"]
    return None


def _spindle_stop_spawn():
    _move_axis_id = _read_current_move_axis_id(read_process_data("manual_move_all_channel")[
        "CHANNEL_MANUAL_MOVE"])
    if _move_axis_id is not None:
        _nc_name = nc_axis_name_from_index(_move_axis_id)
        hand_operator_all_ch.halt("work", _nc_name[0], False)
        gevent.sleep(0.2)
    hand_mode_change.pop_mode()


def force_stop_go_fixed_point():
    """强制结束当前的回固定点过程"""
    global GO_FIXED_POINT_SPAWN  # pylint: disable=W0603
    global GO_FIXED_POINT_STATE
    if GO_FIXED_POINT_SPAWN:
        GO_FIXED_POINT_SPAWN.kill()
        GO_FIXED_POINT_STATE = 2
        GO_FIXED_POINT_SPAWN = None
        gevent.spawn(_spindle_stop_spawn)
